#pragma once
#include "base_interface.h"
#include <rclcpp/rclcpp.hpp>
#include <memory>

namespace io_module {

class CanInterface : public BaseInterface {
public:
    using CallBackU8 = std::function<void(const std_msgs::msg::UInt8::ConstSharedPtr)>;
    using CallBackU16 = std::function<void(const std_msgs::msg::UInt16::ConstSharedPtr)>;

    CanInterface(
        std::shared_ptr<rclcpp::Node>& node,
        const ModuleParam& param
    );

    virtual ~CanInterface();

    bool init() override;
    bool setOutput(const uint8_t &index, const bool &level) override;
    bool readInput(uint64_t &result) override;
    bool setOutputA(const uint8_t &index, const WORD &rawData) override;
    bool readInputA(std::vector<WORD> &result) override;
    void initDIoTopicParam();
    void initAIoTopicParam();
private:
    static rclcpp::QoS get_qos_profile() {
        auto qos = rclcpp::QoS(rclcpp::KeepLast(5))
            .reliable()
            .durability_volatile();

        return qos;
    }

    template <size_t Index>
    void di_byte_sub_callback(const std_msgs::msg::UInt8::ConstSharedPtr msg);

    template <size_t Index>
    void digital8();

    template <size_t Index>
    void ai_word_sub_callback(const std_msgs::msg::UInt16::ConstSharedPtr msg);

    template <size_t Index>
    void analog16();
    
    std::vector<rclcpp::Publisher<std_msgs::msg::UInt8>::SharedPtr> do_byte_pubs_;
    std::vector<rclcpp::Subscription<std_msgs::msg::UInt8>::SharedPtr> di_byte_subs_;
    
    std::vector<rclcpp::Publisher<std_msgs::msg::UInt16>::SharedPtr> ao_word_pubs_;
    std::vector<rclcpp::Subscription<std_msgs::msg::UInt16>::SharedPtr> ai_word_subs_;
};

} // namespace io_module